/*
 * cross.c
 *
 *  Created on: 2023骞�3鏈�20鏃�
 *      Author: 0
 */
#include "zf_common_headfile.h"

// 十字路口状态标志位
enum cross_type_e cross_type = CROSS_NONE;

// 十字状态标志位名称
const char *cross_type_name[CROSS_NUM] = {
        "CROSS_NONE ",
        "CROSS_BEGIN",
        "CROSS_IN   "
};

// 左线左拐标志位,右线右拐标志位
bool far_Lpt0_found_left = false, far_Lpt1_found_right = false;
// 左线左拐下标,右线右拐下标
int far_Lpt0_rpts0s_id_left = 0, far_Lpt1_rpts1s_id_right = 0;

int far_ipts0[FAR_POINTS_MAX_LEN][2]; // 在原图上远左线的坐标
int far_ipts1[FAR_POINTS_MAX_LEN][2]; // 在原图上远右线的坐标
int far_ipts0_num = 0, far_ipts1_num = 0;

float far_rpts0[FAR_POINTS_MAX_LEN][2];  // 逆透视后的左线坐标
float far_rpts1[FAR_POINTS_MAX_LEN][2];  // 逆透视后的右线坐标
int far_rpts0_num = 0, far_rpts1_num = 0;

float far_rpts0b[FAR_POINTS_MAX_LEN][2];  // 滤波后的逆透视左线坐标
float far_rpts1b[FAR_POINTS_MAX_LEN][2];  // 滤波后的逆透视右线坐标
int far_rpts0b_num = 0, far_rpts1b_num = 0;

float far_rpts0s[FAR_POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视左线坐标
float far_rpts1s[FAR_POINTS_MAX_LEN][2];  // 等距采样后的滤波逆透视右线坐标
int far_rpts0s_num = 0, far_rpts1s_num = 0;

float far_rpts0a[FAR_POINTS_MAX_LEN];  // 左线局部角变化率,弧度
float far_rpts1a[FAR_POINTS_MAX_LEN];  // 右线局部角变化率,弧度
int far_rpts0a_num = 0, far_rpts1a_num = 0;

float far_rpts0an[FAR_POINTS_MAX_LEN];  // 左线角变化率非极大抑制
float far_rpts1an[FAR_POINTS_MAX_LEN];  // 右线角变化率非极大抑制
int far_rpts0an_num = 0, far_rpts1an_num = 0;

float far_rptsc0[FAR_POINTS_MAX_LEN][2];  // 左中线
float far_rptsc1[FAR_POINTS_MAX_LEN][2];  // 右中线
int far_rptsc0_num = 0, far_rptsc1_num = 0;

// 存放查找远左右边线的起始坐标
int far_x1 = 0, far_x2 = 0, far_y1 = 0, far_y2 = 0;

// 断路标志位
bool black_road = false;

// 检测前方是否为十字路口
void check_cross (void)
{
    // 记录左右边线拐角状态
    bool Xfound = Lpt0_found_left && Lpt1_found_right;

    if (JOIN_OTHER_STATE && Xfound)
        cross_type = CROSS_BEGIN;
}

void run_cross (void)
{
    // 记录左右边线拐角状态
    bool Xfound = Lpt0_found_left && Lpt1_found_right;

    // 前方为十字
    if (cross_type == CROSS_BEGIN)
    {
        // 对路线进行截断处理,截断至拐角处
        if (Lpt0_found_left)
        {
            rptsc0_num = rpts0s_num = clip((Lpt0_rpts0s_id_left - 2), 0, rpts0s_num);
            track_type = TRACK_LEFT;
        }
        if (Lpt1_found_right)
        {
            rptsc1_num = rpts1s_num = clip((Lpt1_rpts1s_id_right - 2), 0, rpts1s_num);
            track_type = TRACK_RIGHT;
        }

        // 距离左右边线的拐点均存在且左右边线数组长度都小于10
        if ((Lpt0_found_left && Lpt1_found_right) && (rpts0s_num < 10 && rpts1s_num < 10))
        {
            // 状态转换为进入十字
            cross_type = CROSS_IN;
            // 记录远点,将在边线数组中的坐标转换为在原图的坐标
            inv_Perspective(rpts0s[clip((Lpt0_rpts0s_id_left + 5), 0, rpts0a_num)][0], rpts0s[clip((Lpt0_rpts0s_id_left + 5), 0, rpts0a_num)][1], &far_x1, &far_y1);
            inv_Perspective(rpts1s[clip((Lpt1_rpts1s_id_right + 5), 0, rpts1a_num)][0], rpts1s[clip((Lpt1_rpts1s_id_right + 5), 0, rpts1a_num)][1], &far_x2, &far_y2);
        }

        // 若左边线没有左拐点,右边线无有拐点,状态改为无十字
        if (!Xfound && rpts0s_num > 20 && rpts1s_num > 20)
            cross_type = CROSS_NONE;
    }
    else if (cross_type == CROSS_IN)
    {
        // 爬取远左右边线,并处理其形状
        cross_farline();

        // 根据处理后的边线判断循左右哪条边线
        if (far_Lpt1_found_right && far_Lpt0_found_left)
        {
            if (far_rptsc0_num > far_rptsc1_num)
                track_type = TRACK_CROSS_LEFT;
            else
                track_type = TRACK_CROSS_RIGHT;
        }
        else if (far_Lpt1_found_right)
        {
            // 找到右线右拐点,循右线
            track_type = TRACK_CROSS_RIGHT;
        }
        else if (far_Lpt0_found_left)
        {
            // 找到左线左拐点,循左线
            track_type = TRACK_CROSS_LEFT;
        }

        if ((!far_Lpt0_found_left) && (!far_Lpt1_found_right) && rpts1s_num > 20 && rpts0s_num > 20)
        {
            cross_type = CROSS_NONE;
            far_rpts0_num = far_rpts1_num = 0;
        }
    }
}

// 爬取左右远边线,并处理其形状
void cross_farline (void)
{
    uint8 value = 0;
    // 获取远左线
    int x1 = far_x1, y1 = far_y1;
    far_ipts0_num = sizeof(far_ipts0) / sizeof(far_ipts0[0]);
    // 从当前位置向上查找远左线的起始点,白点
    for (; y1 - 1 > block_size; y1--)
    {
        // 查看上方是否为黑色像素
        if (AT_IMAGE(&img_raw, x1, y1 - 1) < thres)
            break;
    }
    // 爬远左线
    findline_lefthand_adaptive(&img_raw, block_size, thres, x1, y1, far_ipts0, &far_ipts0_num, 0);

    // 获取远右线
    int x2 = far_x2, y2 = far_y2;
    far_ipts1_num = sizeof(far_ipts1) / sizeof(far_ipts1[0]);
    // 从当前位置向上查找远右线的起始点,白点
    for (; y2 - 1 > block_size; y2--)
    {
        // 查看上方是否为黑色像素
        if (AT_IMAGE(&img_raw, x2, y2 - 1) < thres)
            break;
    }
    // 爬远右线
    findline_righthand_adaptive(&img_raw, block_size, thres, x2, y2, far_ipts1, &far_ipts1_num, 0);

    // 对远左线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
    if (far_ipts0_num != 0)
    {
        for (int i = 0; i < far_ipts0_num; i++)
        {
            value = ImagePerspective(far_ipts0[i][0], far_ipts0[i][1], &far_rpts0[i][0], &far_rpts0[i][1]);
            if (value == 1)
                break;
            far_rpts0_num = i;
        }
    }
    else
        far_rpts0_num = 0;

    // 对远右线进行进行逆透视变换,将爬到的在原图的坐标转换为在逆透视图上的坐标
    if (far_ipts1_num != 0)
    {
        for (int i = 0; i < far_ipts1_num; i++)
        {
            value = ImagePerspective(far_ipts1[i][0], far_ipts1[i][1], &far_rpts1[i][0], &far_rpts1[i][1]);
            if (value == 1)
                break;
            far_rpts1_num = i;
        }
    }
    else
        far_rpts1_num = 0;

    // 对左右边线进行三角滤波,使得曲线变得平滑,降低异常噪声的干扰
    blur_points(far_rpts0, far_rpts0_num, far_rpts0b, (int) round(line_blur_kernel));
    far_rpts0b_num = far_rpts0_num;
    blur_points(far_rpts1, far_rpts1_num, far_rpts1b, (int) round(line_blur_kernel));
    far_rpts1b_num = far_rpts1_num;

    // 对左右边线进行等距采样,使用线性差值的方式,对曲线上的点进行重新采样,使得采样后的相邻两点之间的距离相等
    far_rpts0s_num = sizeof(far_rpts0s) / sizeof(far_rpts0s[0]);
    resample_points2(far_rpts0b, far_rpts0b_num, far_rpts0s, &far_rpts0s_num, sample_dist * pixel_per_meter);
    far_rpts1s_num = sizeof(far_rpts1s) / sizeof(far_rpts1s[0]);
    resample_points2(far_rpts1b, far_rpts1b_num, far_rpts1s, &far_rpts1s_num, sample_dist * pixel_per_meter);

    // 计算左右边线的局部角变化率
    local_angle_points(far_rpts0s, far_rpts0s_num, far_rpts0a, (int) round(angle_dist / sample_dist));
    far_rpts0a_num = far_rpts0s_num;
    local_angle_points(far_rpts1s, far_rpts1s_num, far_rpts1a, (int) round(angle_dist / sample_dist));
    far_rpts1a_num = far_rpts1s_num;

    // 计算左右边线的局部角变化率非极大抑制
    nms_angle(far_rpts0a, far_rpts0a_num, far_rpts0an, (int) round(angle_dist / sample_dist) * 2 + 1);
    far_rpts0an_num = far_rpts0a_num;
    nms_angle(far_rpts1a, far_rpts1a_num, far_rpts1an, (int) round(angle_dist / sample_dist) * 2 + 1);
    far_rpts1an_num = far_rpts1a_num;

    // 对左右边线进行检测,判断形状
    far_Lpt0_found_left = far_Lpt1_found_right = false;
    far_Lpt0_rpts0s_id_left = far_Lpt1_rpts1s_id_right = 0;

    // 处理左线
    for (int i = 0; i < far_rpts0a_num; i++)
    {
        // 找到该下标的角度与哪两个点组成的
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, far_rpts0s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, far_rpts0s_num - 1);
        float conf = (fabs(far_rpts0a[i]) - (fabs(far_rpts0a[im1]) + fabs(far_rpts0a[ip1])) / 2);
        if (far_rpts0a[i] < 0)
        {
            // 70 < 该点的角度 < 110,左边线向左拐
            if (far_Lpt0_found_left == false && 75. / 180. * PI < conf && conf < 110. / 180. * PI)
            {
                far_Lpt0_rpts0s_id_left = i;
                far_Lpt0_found_left = true;
            }
        }
        // 找到拐点,跳出
        if (far_Lpt0_found_left == true)
            break;
    }

    // 处理右线
    for (int i = 0; i < far_rpts1a_num; i++)
    {
        // 找到该下标的角度与哪两个点组成的
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, far_rpts1s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, far_rpts1s_num - 1);
        float conf = (fabs(far_rpts1a[i]) - (fabs(far_rpts1a[im1]) + fabs(far_rpts1a[ip1])) / 2);
        if (far_rpts1a[i] > 0)
        {
            // 70 < 该点的角度 < 110,右边线向右拐
            if (far_Lpt1_found_right == false && 75. / 180. * PI < conf && conf < 110. / 180. * PI)
            {
                far_Lpt1_rpts1s_id_right = i;
                far_Lpt1_found_right = true;
            }

        }
        // 找到拐点,跳出
        if (far_Lpt1_found_right == true)
            break;
    }

//    if (IfxCpu_acquireMutex(&g_MutexUart))
//    {
//        ips200_show_int(170, 3, far_Lpt0_rpts0s_id_left, 3);
//        ips200_show_int(170, 4, far_Lpt1_rpts1s_id_right, 3);
//        IfxCpu_releaseMutex(&g_MutexUart);
//    }

    // 计算左右中线
    track_leftline(far_rpts0s + (clip((far_Lpt0_rpts0s_id_left + 10), 0, far_rpts0s_num)), \
                   far_rpts0s_num - (clip((far_Lpt0_rpts0s_id_left + 10), 0, far_rpts0s_num)), \
                   far_rptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    far_rptsc0_num = far_rpts0s_num - (clip((far_Lpt0_rpts0s_id_left + 10), 0, far_rpts0s_num));

    track_rightline(far_rpts1s + (clip((far_Lpt1_rpts1s_id_right + 10), 0, far_rpts1s_num)),
                    far_rpts1s_num - (clip((far_Lpt1_rpts1s_id_right + 10), 0, far_rpts1s_num)), \
                    far_rptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    far_rptsc1_num = far_rpts1s_num - (clip((far_Lpt1_rpts1s_id_right + 10), 0, far_rpts1s_num));
}

// 检测断路
void cheak_black_roaad (void)
{
    bool Xfound = Lpt0_found_right && Lpt1_found_left;

    if (Xfound)
    {
        // 置标志位
        black_road = true;

        // 对左右中线进行截断处理
        rptsc0_num = rpts0s_num = clip((Lpt0_rpts0s_id_right - 2), 0, POINTS_MAX_LEN);
        rptsc1_num = rpts1s_num = clip((Lpt1_rpts1s_id_left - 2), 0, POINTS_MAX_LEN);
    }
    else
    {
        black_road = false;
    }
}
